#include"Python.h"
#include "structmember.h"
#include<math.h>
#include"autodealloc_numpy.h"

#define RADIANS(x) ((x) * 0.017453292519943295)

static PyObject * cPlume_getPlumeDepth(PyObject *self, PyObject *args){

	double r; //plume radius (radius of cylinder
	double theta0; //angle of the centre of the plume
	double h_dist; //horizontal distance from the plume
	double v_angle; //angle of scanner
	int i=0;//counter
	PyObject *theta_arr; //numpy array of theta values

	double cx1,cx2,cy1,cy2;


	//parse the arguments passed to the function by Python - no increase in
	//ref count
	if(!PyArg_ParseTuple(args, "ddddO", &r, &theta0, &h_dist, &v_angle, &theta_arr)){
		PyErr_SetString(PyExc_ValueError,"Invalid parameters");
		return NULL;
	}
	//ellipse: x^2/a^2 + y^2/b^2 = 1
    //ellipse params are constrained by the viewing angle
    double cos_v_angle = cos(RADIANS(v_angle));
	double a = r;
    double b = r / cos_v_angle;

    //transform h_dist into the plane of the ellipse
    h_dist = h_dist / cos_v_angle;

    //convert the numpy array into a C array
    //check array is well behaved
    if (! PyArray_ISBEHAVED(theta_arr)){
        PyErr_SetString(PyExc_ValueError,"Theta array must be well behaved");
        return NULL;
    }
    double *theta = (double*) PyArray_DATA(theta_arr);

    int num_entries =(int)PyArray_DIM(theta_arr, 0);
    double c,d;
    double A,B,C, discriminant, sqrt_discr;

    double *result = malloc(num_entries*sizeof(double));
    if (result == NULL){
    	PyErr_SetString(PyExc_ValueError,"Could not alloc results array");
    	return NULL;
    }

    for(i; i<num_entries; i++){
		//use the equation of a straight line x = cy + d we do it
		//this way around because it is possible that theta==theta0, which
		//leads to x1==x2 and an infinite gradient. However, h_dist is never
		//0 and so dx/dy will never be infinite.
		c = tan(RADIANS(theta[i]-theta0));
		d = c * h_dist;

		//substitute the expression for x from the line into the equation of the ellipse
		//and solve using the quadratic equation.
		//quadratic equation -> x = (-B +- sqrt(B^2 - 4AC))/(2A)
		A = ((a * a) / (b * b)) + (c * c);
		B = 2.0 * c * d;
		C = (d * d) - (a * a);

		//mask out scan angles that did not intersect the plume (they have imaginary roots)
		discriminant = (B * B) - (4 * A * C);

		if (discriminant <= 0.0){
			result[i] = 0;
			continue;
		}
		sqrt_discr = sqrt(discriminant);
		//solve for the intersection points of the lines with the ellipse
		cy1 = (-B + sqrt_discr) / (2.0 * A);
		cy2 = (-B - sqrt_discr) / (2.0 * A);

		cx1 = c * cy1 + d;
		cx2 = c * cy2 + d;

		result[i] = sqrt(((cx2 - cx1) *(cx2 - cx1))  + ((cy2 - cy1) *(cy2 - cy1)));
    }

    //create numpy array as return value
    npy_intp dims[1];
    dims[0] = num_entries;
    return createAutoDeallocNumpyArray (1, dims, NPY_FLOAT64, result);

}

//set up the functions to be visible in Python
static PyMethodDef cPlume_methods[] = {
    {"get_path_length", cPlume_getPlumeDepth, METH_VARARGS, ""},
    {NULL, NULL}
};

//set up module to be importable in Python
PyMODINIT_FUNC initcFit(void){
    import_array();
    import_autodealloc_numpy();
    Py_InitModule3("cPlume", cPlume_methods, "");
}
